

/**
  * Autores: Bruna Xavier e Bruno Costa
  * 27/05/2012
  *
  * Código para equalizar a velocidade dos motores.
  * Usa os dois speed encoders.
  * Motor esquerda: Porta digital 2
  * Motor direita: Porta digital 3
  *
 **/

#define DIFF_VEL 20
#define MAX_VEL 255

#define PIN_MOTOR_LEFT_0 6
#define PIN_MOTOR_LEFT_1 7

#define PIN_MOTOR_RIGHT_0 8
#define PIN_MOTOR_RIGHT_1 9

#define ERRO 2

int left_count;
int right_count;

int velocity_motor_left;
int velocity_motor_right;

void setup()
{
  left_count = 0;
  right_count = 0;
  
  velocity_motor_left = MAX_VEL - 4;
  velocity_motor_right = MAX_VEL;
  
  attachInterrupt(0, inc_left, RISING);
  attachInterrupt(1, inc_right, RISING);
  
  setMotorVelocity();
  
  Serial.begin(9600);
}

void loop()
{
  equalizer_motor();
  Serial.print(left_count);
  Serial.print(" ");
  Serial.println(right_count);
//  Serial.print(velocity_motor_right);
  //Serial.print(" ");
  //Serial.println(velocity_motor_left);
}

void equalizer_motor()
{
  if (left_count - right_count > ERRO)
  {
    Serial.println("VERDE MAIOR");
    if (velocity_motor_right < MAX_VEL)
    {
      velocity_motor_right += DIFF_VEL;
    }
    else
    {
      velocity_motor_left -= DIFF_VEL;
    }
    setMotorVelocity();
    //left_count = right_count = 0;
  }
  else if (right_count - left_count > ERRO)
  {
    Serial.println("AZUL MAIOR");
    if (velocity_motor_left < MAX_VEL)
    {
      velocity_motor_left += DIFF_VEL;
    }
    else
    {
      velocity_motor_right -= DIFF_VEL;
    }
    setMotorVelocity();
    //left_count = right_count = 0;
  }
}

/**
  * Usando seguindo o esquema de interrupcao
 **/
void inc_left()
{
  left_count++;
}

/**
  * Usando seguindo o esquema de interrupcao
 **/
void inc_right()
{
  right_count++;
}

void setMotorVelocity()
{
  analogWrite(PIN_MOTOR_LEFT_0, velocity_motor_left);
  analogWrite(PIN_MOTOR_LEFT_1, 0);
  
  analogWrite(PIN_MOTOR_RIGHT_0, velocity_motor_right);
  
  analogWrite(PIN_MOTOR_RIGHT_1, 0);
}

